#include "command_pub.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h" 
#include "nlink_parser/LinktrackAnchorframe0.h"
#include "std_msgs/Int32.h"
#include "nlink_parser/LinktrackTagframe0.h"
#include "nlink_parser/LinktrackNodeframe2.h"
#include "nlink_parser/LinktrackNode2.h"
#include <tf/transform_datatypes.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>

#define NODE_NAME "command_pub"
using namespace std;
#define M_PI 3.14159265358979323846
#define simu_flag  1
float dis_val;

// 新增的全局变量
float avoidance_time = 0.0;  // 避障的停留时间
bool is_avoiding = false;    // 是否正在避障
float avoidance_threshold = 3.0;  // 避障距离阈值
float avoidance_stay_time = 2.0;  // 避障停留时间，单位为秒
Eigen::Vector3d avoidance_direction(1.0, 0.0, 0.0);  // 假设沿着 X 轴方向避开

void cb_uwb(const nlink_parser::LinktrackNodeframe2::ConstPtr& msg)
{
    nlink_parser::LinktrackNodeframe2 uwb_data;
    uwb_data = *msg;
    for (size_t i = 0; i < msg->nodes.size(); ++i) 
    {
        const nlink_parser::LinktrackNode2& node = msg->nodes[i];
        dis_val = node.dis;
        // ROS_INFO("With node[%d] distance : %f \r\n",msg->id,dis_val);
    }

}

// 修改后的避障方向计算函数
Eigen::Vector3d calculate_avoidance_direction(const Eigen::Vector3d& target_position, const Eigen::Vector3d& drone_position)
{
    Eigen::Vector3d to_target = target_position - drone_position;
    to_target.z() = 0;  // 仅在XY平面处理
    to_target.normalize();
    
    // 计算左侧垂直方向（可根据需要改为右侧）
    Eigen::Vector3d avoidance_dir(-to_target.y(), to_target.x(), 0);
    avoidance_dir.normalize();
    
    return avoidance_dir;
}


//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "px4_sender");
    ros::NodeHandle nh("~");
    nh.param("uav_id", uav_id, 0);
    nh.param("neighbour_id1", neighbour_id1, 0);
    nh.param("neighbour_id2", neighbour_id2, 0);
    nh.param("controller_flag", controller_flag, 0);
    // 编队控制参数
    nh.param<float>("k_p", k_p, 0.95);
    nh.param<float>("k_aij", k_aij, 0.1);
    nh.param<float>("k_gamma", k_gamma, 0.1);
    nh.param<float>("k_avoid", k_avoid, 0.1);
    nh.param<double>("pobs_x", pobs_x, 0.0);
    nh.param<double>("pobs_y", pobs_x, 0.0);
    uav_name = "/uav" + std::to_string(uav_id);
    //uav_name ="";
    neighbour_name1 = "/uav" + std::to_string(neighbour_id1);
    neighbour_name2 = "/uav" + std::to_string(neighbour_id2);

   //读取dis_val的值
    ros::Subscriber uwb_sub = nh.subscribe("/nlink_linktrack_nodeframe2", 10, cb_uwb);

    //【订阅】控制指令
    ros::Subscriber Command_sub = nh.subscribe<geometry_msgs::PoseStamped>("/control_command", 10, Command_cb);
    Command_pub = nh.advertise<geometry_msgs::PoseStamped>("/command_pub", 10);
    //【订阅】无人机world坐标[optitrack]
    //ros::Subscriber drone_offboard = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/uav1/pose", 10, offset_cb);
    //【订阅】无人机world坐标[uwb]
    //ros::Subscriber drone_offboard = nh.subscribe<geometry_msgs::PoseStamped>(uav_name +"/uwb/pose", 10, offset_cb);
    // 【订阅】px4状态 - 来自飞控
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>(uav_name +"/mavros/state", 10, state_cb);
    //【订阅】无人机world坐标
    ros::Subscriber drone_state_sub = nh.subscribe<nav_msgs::Odometry>(uav_name +"/drone_state", 10, drone_state_cb);
    // 【订阅】无人机formation
    ros::Subscriber formation_sub = nh.subscribe<state_msg::form>("/formation", 10, form_cb);
    //【发布】控制指令
    //setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local", 10);
    setformation_pub = nh.advertise<state_msg::form>("/set_formation", 10);
    //【发布】控制指令
    //setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local", 10);
    //【发布】控制指令
    // 发送-->px4
    setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>(uav_name +"/mavros/setpoint_raw/local", 10);

    //【订阅】邻居飞机的状态信息
    if(neighbour_name1 != "/uav1" || neighbour_name2 != "/uav1")
    {
        nei1_state_sub = nh.subscribe<nav_msgs::Odometry>(neighbour_name1 + "/drone_state", 10, boost::bind(&nei_state_cb,_1, 0));
        nei2_state_sub = nh.subscribe<nav_msgs::Odometry>(neighbour_name2 + "/drone_state", 10, boost::bind(&nei_state_cb,_1, 1));
    }
    
    float threshold_distance = 3.0; // 距离阈值，小于3停留

    // 建议控制频率 ： 10 - 50Hz, 控制频率取决于控制形式，若控制方式为速度或加速度应适当提高频率
    ros::Rate rate(10.0);
    ros::Duration(3.0).sleep();
    ros::spinOnce();
    Takeoff_position[0] = pos_drone[0];
    Takeoff_position[1] = pos_drone[1];
    Takeoff_position[2] = pos_drone[2];
    cout<<"无人机起飞位置： "<<Takeoff_position[0]<<" "<<Takeoff_position[1]<<" "<<Takeoff_position[2]<<endl;
    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        ros::spinOnce();
        
                // 如果小于3就停留
        if (dis_val < avoidance_threshold) {
            // 实时计算避障方向
            Eigen::Vector3d current_pos(pos_drone[0], pos_drone[1], pos_drone[2]);
            Eigen::Vector3d target_pos(Command_Now.pose.position.x, Command_Now.pose.position.y, Command_Now.pose.position.z);
    
            avoidance_direction = calculate_avoidance_direction(target_pos, current_pos);
    
            // 设置横向避障速度（0.8m/s）并保留10%的前进速度
            Eigen::Vector3d to_target = (target_pos - current_pos).normalized();
            Eigen::Vector3d vel_avoid = avoidance_direction * 0.8f + to_target * 0.1f;
    
            // 保持高度控制
            state_des[2] = Command_Now.pose.position.z;
    
            // 发送速度指令
            vel_des[0] = vel_avoid.x();
            vel_des[1] = vel_avoid.y();
            vel_des[2] = 0.0;
            yaw_des = atan2(to_target.y(), to_target.x());  // 保持面向目标方向
    
            send_vel_xy_pos_z_setpoint(state_des, vel_des, yaw_des);
            is_avoiding = true;
            
        }else{
            // 大于3米，退出避障，继续飞向目标位置
            if (is_avoiding) 
            {
                // 退出避障时进行位置校正
                state_sp = Eigen::Vector3d(pos_drone[0], pos_drone[1], Command_Now.pose.position.z);
                send_pos_setpoint(state_sp, yaw_sp);
                is_avoiding = false;
            }
            if(controller_flag == 0)
            {
                if(time_flag >= 0 && time_flag <= 100 )
                    {
                        state_sp = Eigen::Vector3d(Takeoff_position[0],Takeoff_position[1],Command_Now.pose.position.z);
                        yaw_sp = quaternionToYaw(q_fcu);
                        yaw_sp = yaw_sp * (M_PI / 180.0);initial_velocity[0] = std::sqrt(2 * acceleration * distance_to_target[0]);
                        send_pos_setpoint(state_sp, yaw_sp);
                    }
                else if(time_flag > 100 && time_flag <= 200)
                    {
                        x_sp = Takeoff_position[0] + (Command_Now.pose.position.x -  Takeoff_position[0]) * (time_flag-100) /100;
                        y_sp = Takeoff_position[1] + (Command_Now.pose.position.y -  Takeoff_position[1]) * (time_flag-100) /100;
                        state_sp = Eigen::Vector3d(x_sp,y_sp,Command_Now.pose.position.z);
                        yaw_sp = quaternionToYaw(q_fcu);
                        yaw_sp = yaw_sp * (M_PI / 180.0);
                        send_pos_setpoint(state_sp, yaw_sp);
                    }
                else if(time_flag > 200)
                    {
                        ros::spinOnce();
                        state_sp = Eigen::Vector3d(Command_Now.pose.position.x,Command_Now.pose.position.y,Command_Now.pose.position.z);
                        error_flag = error_check(state_sp,state_des);
                        if(error_flag == 1)
                        {
                            state_sp = Eigen::Vector3d(Command_Now.pose.position.x,Command_Now.pose.position.y,Command_Now.pose.position.z);
                            yaw_sp = quaternionToYaw(q_fcu);
                            yaw_sp = yaw_sp * (M_PI / 180.0);
                        }
                        else{
                            time_flag = 0;
                            ros::spinOnce();
                            Takeoff_position[0] = offset_position[0];
                            Takeoff_position[1] = offset_position[1];
                        }
                        send_pos_setpoint(state_sp, yaw_sp);
                    }
                else{
                    ROS_ERROR("time_flag error");
                }
                setformation_pub.publish(formation_msg);
                Command_pub.publish(Command_Now);
            }

            else if(controller_flag == 1)
            {
                if(time_flag >= 0 && time_flag <= 100)
                {
                    state_des[0] = Takeoff_position[0];
                    state_des[1] = Takeoff_position[1];
                    state_des[2] = Command_Now.pose.position.z;
                    send_pos_setpoint(state_des,0);
                }
                else
                {
                yaw_sp = quaternionToYaw(q_fcu);
                yaw_sp = yaw_sp * (M_PI / 180.0);
                formation_separation = formation_utils::get_formation_separation(formation_msg.swarm_shape, formation_msg.swarm_size);
                yita = 1/ ((float)swarm_num * k_aij + k_p);
                state_des[0] = 0.0;
                state_des[1] = 0.0;
                state_des[2] = Command_Now.pose.position.z + formation_separation(uav_id-1,2);
                // 计算当前位置与目标点之间的距离
                distance_to_target[0] = Command_Now.pose.position.x + formation_separation(uav_id-1,0) - pos_drone[0];
                distance_to_target[1] = Command_Now.pose.position.y + formation_separation(uav_id-1,1) - pos_drone[1];
                // 计算以给定加速度达到目标点所需的初速度
                initial_velocity[0] = (distance_to_target[0] > 0 ? 1 : -1)*std::sqrt(2 * acceleration * abs(distance_to_target[0]));  
                initial_velocity[1] = (distance_to_target[1] > 0 ? 1 : -1)*std::sqrt(2 * acceleration * abs(distance_to_target[1]));
                //避障
                // if(pobs_x !=0 || pobs_y !=0){
                //     avoid.pos_drone_avoid<<pos_drone[0],pos_drone[1];
                //     avoid.pos_target<<Command_Now.pose.position.x + formation_separation(uav_id-1,0),Command_Now.pose.position.y + formation_separation(uav_id-1,1);
                //     avoid.pos_obs1<<pobs_x,pobs_y;
                //     avoid.process();
                // }
                // else{
                //     avoid.v0.setZero();
                // }
                //cout<<"initial_velocity:"<<initial_velocity[0]<<","<<initial_velocity[1]<<endl;
                vel_des[0] = - yita * k_aij * ( vel_nei[0][0] - k_gamma *((pos_drone[0] - pos_nei[0][0]) - ( formation_separation(uav_id-1,0) -  formation_separation(neighbour_id1-1,0)))) 
                        - yita * k_aij * ( vel_nei[1][0] - k_gamma *((pos_drone[0] - pos_nei[1][0]) - ( formation_separation(uav_id-1,0) -  formation_separation(neighbour_id2-1,0))))
                        + yita * k_p * ( initial_velocity[0] - k_gamma * (pos_drone[0] - Command_Now.pose.position.x - formation_separation(uav_id-1,0)));// + k_avoid * avoid.v0[0];
                vel_des[1] = - yita * k_aij * ( vel_nei[0][1] - k_gamma *((pos_drone[1] - pos_nei[0][1]) - ( formation_separation(uav_id-1,1) -  formation_separation(neighbour_id1-1,1)))) 
                        - yita * k_aij * ( vel_nei[1][1] - k_gamma *((pos_drone[1] - pos_nei[1][1]) - ( formation_separation(uav_id-1,1) -  formation_separation(neighbour_id2-1,1))))
                        + yita * k_p * ( initial_velocity[1] - k_gamma * (pos_drone[1] - Command_Now.pose.position.y - formation_separation(uav_id-1,1)));// + k_avoid * avoid.v0[1];
                vel_des[2] = 0.0;
                acc_des << 0.0, 0.0, 0.0;
                yaw_des = yaw_sp + formation_separation(uav_id-1,3); 
                send_vel_xy_pos_z_setpoint(state_des, vel_des, yaw_des);
                } 
                setformation_pub.publish(formation_msg);
                Command_pub.publish(Command_Now);
            }
            else if(controller_flag == 2)
            {
                if(time_flag >= 0 && time_flag <= 500)
                {
                state_des[0] = Takeoff_position[0];
                state_des[1] = Takeoff_position[1];
                state_des[2] = Command_Now.pose.position.z;  
                yaw_sp = 0;
                }
                else
                {      
                // formation_separation = formation_utils::get_formation_separation(formation_msg.swarm_shape, formation_msg.swarm_size);
                // state_des[0] = Command_Now.pose.position.x + formation_separation(uav_id-1,0);
                // state_des[1] = Command_Now.pose.position.y + formation_separation(uav_id-1,1);
                // state_des[2] = Command_Now.pose.position.z + formation_separation(uav_id-1,2);
                // //cout<<"pos_target: "<<state_des[0]<<" "<<state_des[1]<<" "<<state_des[2]<<endl;
                state_des[0] = Command_Now.pose.position.x;
                state_des[1] = Command_Now.pose.position.y;
                state_des[2] = Command_Now.pose.position.z;
                Eigen:Vector3d vel_des(0,0,0);
                ux = MFAC_X(state_des[0] , pos_drone[0] , vel_drone[0]);
                uy = MFAC_Y(state_des[1] , pos_drone[1] , vel_drone[1]);
                acc_des[0] = ux ;
                acc_des[1] = uy ;
                acc_des[2] = 0 ;
                yaw_sp = 0;
                }
                setformation_pub.publish(formation_msg);
                Command_pub.publish(Command_Now);
                send_xyz_all_setpoint(state_des,vel_des,acc_des,yaw_sp);
            } 
        }
      
        if(mode == "OFFBOARD")
        {
            time_flag += 1;
        }
        rate.sleep();
    }
    return 0;
}
